#include <c28.h>

#define DEBUG_ON 1
#define TOUT(s, t, p) do{if(s > 0) p;}while(0)  //for avoid header file collisions

#define ERR_NOMEM           -2
#define ERR_NOSPACE         -3
#define ERR_NULL_HEAD       -4
#define ERR_DEL_PHOLE_HEAD  -5

typedef unsigned int u32;
typedef u32 phys_nbytes;

int nr_avail_holes = 0;

typedef struct memory{
    phys_nbytes base;//bytes
    phys_nbytes size;//bytes
}memory_t;

#define SIZE_5M        0x500000
#define AVAIL_MEM_BASE (SDRAM_END-SIZE_5M)
#define AVAIL_MEM_SIZE (SIZE_5M)

typedef struct hole{//
    struct hole *h_next;
    struct hole *h_prev;
    phys_nbytes h_base;//where does the hole begin?
    phys_nbytes h_len;//how big is the hole?
}hole_t;

//the array only be a storage usage for this time no memory functions
#define NR_HOLES 1024
static hole_t hole[NR_HOLES];//all the holes of the system are collected here;
static hole_t *phole_head;	
#define NIL_HOLE ((hole_t *) 0)

#if 0
#define align_up(a, n)	(((u32)(a) + ((u32)(n) - 1)) & ~((u32)(n) - 1))
#define align_down(val, align_val) ((val) &~ ((align_val) - 1))
#else
#define align_up(a, n)   ((u32)a)
#define align_down(a, n) ((u32)a)
#endif

//=============================================
extern "C"{
int printf(const char *fmt, ...);
}
static inline void insert_hole_prev(hole_t *hp, hole_t *hp_prev);
static inline void insert_hole_after(hole_t *hp, hole_t * hp_after);
static inline int del_hole(struct hole *hp);
static inline void merge(hole_t *hp);
static void dump_hole_list();
//=============================================

static void init_hole_array(void)
{
    int i;
    for(i = 0; i < NR_HOLES; i++){
        hole[i].h_next = NIL_HOLE;
        hole[i].h_prev = NIL_HOLE;
        hole[i].h_base = 0;
        hole[i].h_len = 0;
    }
    return;
}

///param: avail_mem:  list of free memory avail_mem 
//free:		 memory size summaries 
static void mem_init(void)
{
    memory_t avail_mem; 
    static int init_mem_over = 0;
    if(0 == init_mem_over){
        avail_mem.base = AVAIL_MEM_BASE;
        avail_mem.size = AVAIL_MEM_SIZE;
        TOUT(DEBUG_ON, 1, printf("\navail_mem.base=%x", avail_mem.base));
        TOUT(DEBUG_ON, 1, printf("\navail_mem.size=%x", avail_mem.size));

        init_hole_array();
        hole[0].h_base = avail_mem.base;
        hole[0].h_len = avail_mem.size;//can not bigger than Real Ram Size
        hole[0].h_prev = hole[0].h_next = &hole[0];
        phole_head = &hole[0];
        nr_avail_holes = 1;

        printf("\nHi I am here in mem_init()");
        TOUT(DEBUG_ON, 1, printf("\nhole[0].h_base=0x%x", hole[0].h_base));
        TOUT(DEBUG_ON, 1, printf("\nhole[0].h_len=0x%x", hole[0].h_len));
        init_mem_over = 1;
    }

    return;
}

static int alloc_mem_nbytes(phys_nbytes nbytes)
{
    register struct hole *hp, *prev_ptr;
    phys_nbytes old_base;
    int i = 0;

    TOUT(DEBUG_ON, 1, printf("\nnbytes=0x%x", nbytes));

    hp = phole_head;
    while(i++ < nr_avail_holes){
        if(hp->h_len >= nbytes){
            old_base = hp->h_base;
            hp->h_base += nbytes;
            hp->h_len -= nbytes;

            TOUT(DEBUG_ON, 1, printf("\nNow alloc:"));
            TOUT(DEBUG_ON, 1, printf("\nhp->h_base=0x%x", hp->h_base));
            TOUT(DEBUG_ON, 1, printf("\nhp->h_len=0x%x", hp->h_len));

            if(0 == hp->h_len){
                del_hole(hp);
            }
            else{
                dump_hole_list();
                return old_base;
            }
        }
        prev_ptr = hp;
        hp = hp->h_next;
    }
    return ERR_NOMEM;
}

char *malloc(unsigned int len)
{
    if(0 == len)
        return ((char *)0);
    mem_init();
    dump_hole_list();
    return (char *)alloc_mem_nbytes((phys_nbytes) len);
}

static int free_mem_nbytes(phys_nbytes base, phys_nbytes nbytes)
{
    register hole_t *hp = phole_head;
    hole_t *inserted_hp = NIL_HOLE;
    int i = 0;

    TOUT(DEBUG_ON, 1, printf("\nfree_mem_nbytes(%x, %d):", base, nbytes));
    if(0 == nbytes) return 0;

    if(NIL_HOLE == phole_head)
        return ERR_NULL_HEAD;

    while(i++ < NR_HOLES){
        if(0 != hole[i - 1].h_len)
            continue;
        hole[i-1].h_base = base;//first find a place to sit down
        hole[i-1].h_len = nbytes;
        inserted_hp = &hole[i - 1];
        TOUT(DEBUG_ON, 1, printf("\ninserted_hp=%x", inserted_hp));
        TOUT(DEBUG_ON, 1, printf("\ninserted_hp->h_base=%x inserted_hp->h_len=%d", inserted_hp->h_base, inserted_hp->h_len));
        TOUT(DEBUG_ON, 1, printf("\nhole[%d-1].h_base=%8x ,hole[%d-1].h_len=%d", i, (phys_nbytes)base, i, nbytes));
        break;
    }
    if(i >= NR_HOLES){
        return ERR_NOSPACE;
    }


    if(base < phole_head->h_base){//first
        TOUT(DEBUG_ON, 1, printf("\nInsert into the first:"));
        insert_hole_prev(&hole[i - 1], phole_head);
        phole_head = hp = &hole[i - 1];
        printf("\nNow Let's See the 2 node phole_head list:");
        dump_hole_list();
    }
    else{
        hp = phole_head;
        while(base > hp->h_base){
            hp = hp->h_next;
            if(phole_head == hp)
                break;
        }
        if(phole_head != hp){ //insert here : middle
            TOUT(DEBUG_ON, 1, printf("\nInsert into the middle:"));
            insert_hole_prev(&hole[i - 1], hp);
        }
        else{//the biggest base,insert at the last
            TOUT(DEBUG_ON, 1, printf("\nInsert into the last:"));
            insert_hole_after(&hole[i - 1], phole_head->h_prev);
        }
    }

    nr_avail_holes++;
    merge(inserted_hp);
    return 0;  
}

void free(void *base, unsigned int len)
{
    TOUT(DEBUG_ON, 1, printf("\nfree(%8x, %d)", (phys_nbytes)base, len));
    free_mem_nbytes((phys_nbytes) base, (phys_nbytes) len);
    dump_hole_list();
}

static inline void merge(hole_t *hp)
{
    TOUT(DEBUG_ON, 1, printf("\nmerge() is called.!\n"));
    if(hp->h_base + hp->h_len == hp->h_next->h_base){
        TOUT(DEBUG_ON, 1, printf("\nmerge with next !\n"));
        TOUT(DEBUG_ON, 1, printf("\nhp->h_base =%x hp->h_len=%x hp->h_next->h_base=%x", hp->h_base, hp->h_len, hp->h_next->h_base));
        hp->h_len += hp->h_next->h_len;
        del_hole(hp->h_next);
    }
    if(hp->h_base == hp->h_prev->h_base + hp->h_prev->h_len){
        TOUT(DEBUG_ON, 1, printf("\nmerge with prev!\n"));
        TOUT(DEBUG_ON, 1, printf("\nhp->h_prev->h_base =%x hp->h_prev->h_len=%x hp->h_base=%x", hp->h_prev->h_base, hp->h_prev->h_len, hp->h_base));
        hp->h_prev->h_len += hp->h_len;
        del_hole(hp); 
    }
}

static inline int del_hole(struct hole *hp)
{
    TOUT(DEBUG_ON, 1, printf("\ndel_hole() is called.!\n"));
    if(hp == phole_head){//delete the first node
        if(hp->h_prev->h_base < hp->h_base){//Check 
            TOUT(DEBUG_ON, 1, printf("\ndel_hole() del the header node.\n"));
            phole_head = hp->h_prev;
        }
        else{
            TOUT(DEBUG_ON, 1, printf("\nError: del phole_head wrong."));
            return ERR_DEL_PHOLE_HEAD;
        }
    }

    hp->h_prev->h_next = hp->h_next;
    hp->h_next->h_prev = hp->h_prev;
    hp->h_len = 0;
    nr_avail_holes--;

    return 0;
}

static inline void insert_hole_prev(hole_t *hp, hole_t *hp_prev)
{
    hp->h_next = hp_prev;
    hp->h_prev = hp_prev->h_prev;
    hp_prev->h_prev->h_next = hp;
    hp_prev->h_prev = hp;
}

static inline void insert_hole_after(hole_t *hp, hole_t * hp_after)
{
    hp->h_next = hp_after->h_next;
    hp->h_prev = hp_after;
    hp_after->h_next->h_prev = hp;
    hp_after->h_next = hp;
}

#if DEBUG_ON == 1
static void dump_hole_list()
{
    hole_t *p;
    int i = 0;
    printf("\n========================");
    printf("\n\nNow dump_hole_list()");
    p = phole_head;
    if(p->h_next == phole_head){
        printf("\nphole_head: ");
        printf("\np->h_base = %x  p->h_len = %x", p->h_base, p->h_len);
        return;
    }
    printf("\nHey, not ONLY phole_head:");
    while(p->h_next != phole_head){
        printf("\n%d): p->h_base = %x  p->h_len = %x", i++, p->h_base, p->h_len);
        p = p->h_next;
    }
    if(p->h_next == phole_head){
        printf("\nOh,The Last hole:");
        printf("\n%d): p->h_base = %x  p->h_len = %x", i++, p->h_base, p->h_len);
    }
    printf("\n========================\n");
    return;
}
#else
static void dump_hole_list()
{
    return;
}
#endif
